#include <ros/ros.h>
#include <sensor_msgs/Joy.h>
#include <sstream>
#include <string>
#include <vector>  //for std::vector
#include <stdio.h>
#include "time.h"
#include <std_msgs/String.h>;
#include <math.h>
#include <iostream>
#include <std_msgs/Int16MultiArray.h>

#include <algorithm>    // std::sort

// CURL
#include <cstdlib>
#include <curlpp/cURLpp.hpp>
#include <curlpp/Easy.hpp>
#include <curlpp/Options.hpp>
#include <curlpp/Exception.hpp>
#include <curlpp/Infos.hpp>
#include <ostream>
#include <istream>
// JSON
#include "jsoncpp/include/json/json.h"

using namespace std;

int LED[15];
std_msgs::Int16MultiArray LED_msg;

  ros::Publisher LED_pub;


//CURL
curlpp::Cleanup cleaner;
curlpp::Easy request;
Json::Reader reader;
Json::Value value;

std::vector<std::string> out_array;
std::vector<std::string> destination_ship;
std::vector<int> destination_ship_int;
    int queue_num;
int queue_old=0;
std::vector<int> queueid_array;
std::vector<int> boxid_array;
std::vector<int> drugid_array;
std::vector<int> packagesize_array;
std::vector<int> queueServicePoint_array;

std::vector<int> package_out_array;
int package_out_old;
std::vector<int> window_out_array;
std::vector<int> slotid_out_array;


int chk_connection;

        int x_robot = 0;
        int y_robot = 0;
        int x_start = 0;
        int y_start = 0;
        int obstacle =0;
        int x_goal = 1;
        int y_goal = 5;
        int heading = 270;
        int job_finished[5];

        int onCloseList = 10;

        // A* Algorithm  variable
        const int numberPeople = 1;
        const int found = 1;
        const int nonexits = 2;
        const int noPath = 3;
        const int walkable = 0;
        const int unwalkable = 1;
        const int notstarted = 0;
        const int notfinished = 0;

        int agv_state;


        int walkability[501][501];
        int openList[250002];  // (x * y) + 2  ; (mapHeight * mapWidth ) + 2
        int whichList[501][501];               // <<< problem
        int openX[250002];
        int openY[250002];
        int parentX[501][501];
        int parentY[501][501];
        int Fcost[250002];
        int Gcost[501][501];
        int Hcost[250002];
        int pathLenght[numberPeople + 1];
        int pathLocation[numberPeople + 1];

        //Path reading variables
        int pathStatus[numberPeople+1];
        int xPath[numberPeople+1];
        int yPath[numberPeople + 1];
        int pathBank[numberPeople + 1][250002];


        int pathNextX[20],pathNextY[20];
        int pathNextXUSE,pathNextYUSE;

        int Destination[5];
        int robot_position;
        int robot_position_old;



       // int count;
        int mouse_move =0;
        int path_update = 0;

        int find_stop = 0;


        int queueid;
        int boxid;
        int drugid;
        int packagesize;

        // Package socket
        int package_id;
        int package_size;
        int package_ward;
        int package_count = 0;   // number of package in socket
        int socket_status = 0;   // 1 = full
        int tempAAA=0;
        int tempAA = 0;
        int tempA = 0;
        int tempBB = 0;
        int tempB = 0;
        int tempCC = 0;
        int tempC = 0;
        int LimitSw[16];
        int package_complete=0;
        int shelfA_status=0;
        int shelfB_status=0;
        int shelfC_status=0;
        int qno_old=0;
        int slot_active[12];
        int slot_num; // for select slot number
        int bb;   // for select shelf number


         int queueServicePoint;


         int servicepointid_old =0;

         std::vector<int> tee;      // for keep service point id of shelf
         std::vector<int> shelfA;
         std::vector<int> shelfB;
         std::vector<int> shelfC;




        // User interface
        int start_button=0;  // 1 : pressed
        int start_enable=0;
        int reset_button=0;
        int agv_mode=0;  // 1 : Auto    0 : Manual
        int EmergencySw=0;
        int doorSwA=0;
        int doorSwB=0;
        int doorSwC=0;


void updateDoorstatus(int robotid,int status);
void updateWindow(int slotnum,int status);
void updateSlotAGV(int slotnum,int drugid,int status,int slottypeid);

int Findpath(int PathfinderID) // FindPath
{

    int count = 0;


            int onOpenList = 0, parentXval = 0, parentYval = 0;
            int a = 0, b = 0, m = 0, u = 0, v = 0, temp = 0, corner = 0;
            int numberofOpenListItem = 0;
            int addedGcost=0,tempGcost=0,path=0;
            int tempx=0, pathX=0, pathY=0, cellPosition=0, newOpenListItemID = 0;

            // 1. Quick path check
            if (x_start == x_goal && y_start == y_goal && pathLocation[PathfinderID] > 0)
            {
                return found;
            }
            if (x_start == x_goal && y_start == y_goal && pathLocation[PathfinderID] == 0)
            {
                return nonexits;
            }

            if (walkability[x_goal][y_goal] == unwalkable)          // if Obstacle on target
            {
                return noPath;
            }

            // 2. Reset variable
            //if (onCloseList > 100000000) //reset whichList occasionally
            //{
                for (int x = 0; x < 300; x++)
                {
                    for (int y = 0; y < 300; y++)
                        whichList[x][y] = 0;
                }
                onCloseList = 10;
            //}

            onCloseList = 2;        // กำหนดให้ตำแหน่งที่เคยค้นแล้วมีค่าเท่ากับ 2
            onOpenList = 1;         // กำหนดให้ตำแหน่งที่เจอแล้วมีค่าเท่ากับ 1
            pathLenght[PathfinderID] = notstarted;
            pathLocation[PathfinderID] = notstarted;
            Gcost[x_start][y_start] = 0;

            // 3. Add start location to open list of squares to be checked.
            numberofOpenListItem = 1;
            openList[1] = 1;                // [1,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx]               Binary Heap
            openX[1] = x_start;             // [x_start,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx]         Binary Heap
            openY[1] = y_start;             // [y_start,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx,xx]         Binary Heap

            // Loop untill a path is found
            do
            {
                if (numberofOpenListItem != 0) // if numberofOpenListItem != 0
                {
                    parentXval = openX[openList[1]];            // นำค่าแรกสุดมาใส่ในตำหน่งนี้เพื่อทำการ search
                    parentYval = openY[openList[1]];

                    whichList[parentXval][parentYval] = onCloseList;        // เก็บว่าตำแหน่งนี้ได้ทำการ search มาแล้ว

                    numberofOpenListItem = numberofOpenListItem - 1;

                    openList[1] = openList[numberofOpenListItem + 1];
                    v = 1;

                    // Repeat the following untill the new item in slot#1       // Binaty Heap
                    do
                    {
                        u = v;
                        if (2 * u + 1 <= numberofOpenListItem)           // check number of open list @least 3 numbers
                        {
                            if (Fcost[openList[1]] >= Fcost[openList[2 * u]])
                            {
                                v = 2 * u;
                            }
                            if (Fcost[openList[1]] >= Fcost[openList[2 * u + 1]])
                            {
                                v = 2 * u + 1;
                            }
                        }
                        else
                        {
                            if (2 * u <= numberofOpenListItem)   // have 1 child
                            {
                                if (Fcost[openList[1]] >= Fcost[openList[2 * u]])
                                {
                                    v = 2 * u;
                                }
                            }
                        }

                        if (u != v)
                        {
                            temp = openList[u];
                            openList[u] = openList[v];
                            openList[v] = temp;
                        }
                        else
                            break;

                    }// Recorder the binary heap
                    while (find_stop != 1);

                    // check the adjacent square , Add adjacent square child square to open list
                    for (b = parentYval - 1; b <= parentYval + 1; b++)
                    {
                        for (a = parentXval - 1; a <= parentXval + 1; a++)
                        {
                            //if(a != -1 && b!= -1 && a != 50 && b!= 50)
                            if (a > 0 && b > 0 && a != 90 && b != 90)
                            {
                                if (whichList[a][b] != onCloseList)
                                {
                                    if (walkability[a][b] != unwalkable)
                                    {
                                        corner = walkable;
                                        if (a == parentXval - 1)
                                        {
                                            if (b == parentYval - 1)
                                            {
                                                if (walkability[parentXval - 1] [parentYval] == unwalkable || walkability[parentXval][parentYval - 1] == unwalkable)
                                                    corner = unwalkable;
                                            }
                                            else if (b == parentYval + 1)
                                            {
                                                if (walkability[parentXval] [parentYval + 1] == unwalkable || walkability[parentXval - 1] [parentYval] == unwalkable)
                                                    corner = unwalkable;
                                            }
                                        }
                                        else if (a == parentXval + 1)
                                        {
                                            if (b == parentYval - 1)
                                            {
                                                if (walkability[parentXval] [parentYval - 1] == unwalkable || walkability[parentXval + 1] [parentYval] == unwalkable)
                                                    corner = unwalkable;
                                            }
                                            else if (b == parentYval + 1)
                                            {
                                                if (walkability[parentXval + 1] [parentYval] == unwalkable || walkability[parentXval] [parentYval + 1] == unwalkable)
                                                    corner = unwalkable;
                                            }
                                        }

                                        if (corner == walkable)
                                        {
                                            if (whichList[a][b] != onOpenList)
                                            {
                                                // Create new item in the binary heap
                                                newOpenListItemID = newOpenListItemID + 1;
                                                m = numberofOpenListItem + 1;     // นำไปไว้ตัวสุดท้ายของ list
                                                openList[m] = newOpenListItemID;
                                                openX[newOpenListItemID] = a;
                                                openY[newOpenListItemID] = b;

                                                if (fabs(a - parentXval) == 1 && fabs(b - parentYval) == 1)
                                                    addedGcost = 2000;
                                                else
                                                    addedGcost = 10;
                                                Gcost[a][b] = Gcost[parentXval] [parentYval] + addedGcost;

                                                //Figure out its H and F costs and parent
                                                Hcost[openList[m]] = 2000 * (fabs(a - x_goal) + fabs(b - y_goal));
                                                Fcost[openList[m]] = Gcost[a][b] + Hcost[openList[m]];
                                                parentX[a][b] = parentXval;
                                                parentY[a][b] = parentYval;


                                                // Use Add item in binary heap
                                                while (m != 1) //While item hasn't bubbled to the top (m=1)
                                                {
                                                    //Check if child's F cost is < parent's F cost. If so, swap them.
                                                    if (Fcost[openList[m]] <= Fcost[openList[m / 2]])
                                                    {
                                                        temp = openList[m / 2];
                                                        openList[m / 2] = openList[m];
                                                        openList[m] = temp;
                                                        m = m / 2;
                                                    }
                                                    else
                                                        break;
                                                }

                                                numberofOpenListItem = numberofOpenListItem + 1;

                                                //Change whichList to show that the new item is on the open list.
                                                whichList[a][b] = onOpenList;

                                            }//(whichList[a, b] != onCloseList)

                                            else
                                            {
                                                if (fabs(a - parentXval) == 1 && fabs(b - parentYval) == 1)
                                                    addedGcost = 2000;
                                                else
                                                    addedGcost = 10;

                                                tempGcost = Gcost[parentXval][parentYval] + addedGcost;

                                                if (tempGcost < Gcost[a][b])
                                                {
                                                    parentX[a][b] = parentXval;
                                                    parentY[a][b] = parentYval;
                                                    Gcost[a][b] = tempGcost;

                                                    // Loop untill found item
                                                    for (int x = 1; x <= numberofOpenListItem; x++)
                                                    {
                                                        if (openX[openList[x]] == a && openY[openList[x]] == b)
                                                        {
                                                            Fcost[openList[x]] = Gcost[a][b] + Hcost[openList[x]];

                                                            m = x;

                                                            while (m != 1) //While item hasn't bubbled to the top (m=1)
                                                            {
                                                                //Check if child is < parent. If so, swap them.
                                                                if (Fcost[openList[m]] < Fcost[openList[m / 2]])
                                                                {
                                                                    temp = openList[m / 2];
                                                                    openList[m / 2] = openList[m];
                                                                    openList[m] = temp;
                                                                    m = m / 2;
                                                                }
                                                                else
                                                                    break;
                                                            }
                                                            break;
                                                        }//if openX and openY check
                                                    }// for x <= numberofOpenlistItem
                                                }// if temp Gcost
                                            }
                                        }//(corner == walkable)

                                    }// check unwalkable
                                }// do if whileList[a,b] != closelist
                          } // check a , b for out of map range
                        }//parentXval

                    }//parentYval

                }// if numberofOpenListItem != 0
                else
                {
                    path = nonexits;
                    break;
                }

                if (whichList[x_goal][y_goal] == onOpenList)
                {
                    path = found;
                    break;
                }
            }// Loop untill a path is found
            while(true);

            if (path == found)
            {
                pathX = x_goal;
                pathY = y_goal;

                do
                {
                    tempx = parentX[pathX][pathY];
                    pathY = parentY[pathX][pathY];
                    pathX = tempx;

                    pathLenght[PathfinderID] = pathLenght[PathfinderID] + 1;

                }
                while (pathX != x_start || pathY != y_start);

                pathX = x_goal;
                pathY = y_goal;

                cellPosition = pathLenght[PathfinderID] * 2;//start at the end

                do
                {
                    cellPosition = cellPosition - 2;//work backwards 2 integers


                    count++;
                    //d.Look up the parent of the current cell.
                    tempx = parentX[pathX][pathY];
                    pathY = parentY[pathX][pathY];
                    pathX = tempx;

                    pathNextX[count] = pathX;
                    pathNextX[count] = pathY;
                     ROS_INFO("Path : %d,%d",pathX,pathY);

                    if(pathLenght[1]-count == 1)
                    {
                        pathNextXUSE = pathX;
                        pathNextYUSE = pathY;
                        //ROS_INFO("Next Path is: %d,%d",pathNextXUSE,pathNextYUSE );

                    }



                }
                while (pathX != x_start || pathY != y_start);

            }

            return path;
}// FindPath

void QRCallback(const std_msgs::StringConstPtr &p)
{
         if(p->data == "start")
         {
            robot_position = 1;
            ROS_INFO("Robot position : START");
         }
         if(p->data == "node2")
         {
            robot_position = 2;
            ROS_INFO("Robot position : NODE2");
         }
         if(p->data == "node3")
         {
            robot_position = 3;
            ROS_INFO("Robot position : NODE3");
         }
         if(p->data == "node4")
         {
            robot_position = 4;
            ROS_INFO("Robot position : NODE4");
         }
         if(p->data == "node5")
         {
            robot_position = 5;
            ROS_INFO("Robot position : INTERSECT");
         }

}

void InteracSwCallback(const std_msgs::Int16MultiArray& msg)
{
    if(msg.data[3] == 0)
    {
        start_button = 1;
    }
    else if(msg.data[3] == 1)
    {
        start_button = 0;
    }
    if(msg.data[2] == 0)
    {
        reset_button = 1;
    }
    else if(msg.data[2] == 1)
    {
        reset_button = 0;
    }

    if(msg.data[6] == 0)
    {
        agv_mode = 1;  // Manual
    }
    else if(msg.data[6] == 1)
    {
        agv_mode = 0;   // Auto
    }
}

void LimitSwCallback(const std_msgs::Int16MultiArray& msg)
{
    for(int sw=0;sw<=15;sw++)
        LimitSw[sw] = msg.data[sw];

    // SHelf update
    if(LimitSw[12] == 1)
        updateWindow(1,1);
    else if(LimitSw[12] == 0)
        updateWindow(1,0);
    if(LimitSw[13] == 1)
        updateWindow(2,1);
    else if(LimitSw[13] == 0)
        updateWindow(2,0);
    if(LimitSw[14] == 1)
        updateWindow(3,1);
    else if(LimitSw[14] == 0)
        updateWindow(3,0);


    if(LimitSw[0]==0 && LimitSw[1]==0 && LimitSw[2]==0 && LimitSw[3]==0)
        shelfA_status = 1; // Full
    if(LimitSw[4]==0 && LimitSw[5]==0 && LimitSw[6]==0 && LimitSw[7]==0)
        shelfB_status = 1; // Full
    if(LimitSw[8]==0  && LimitSw[9]==0 && LimitSw[10]==0 && LimitSw[11]==0)
        shelfC_status = 1; // Full

int SlotA1,SlotA2,SlotB1,SlotB2; // ShelfA
int SlotA3,SlotA4,SlotB3,SlotB4; // ShelfB
int SlotA5,SlotA6,SlotB5,SlotB6; // ShelfC

    if(LimitSw[0] == 1)
        SlotA1 = 1;
    else if (LimitSw[0] == 0)
        SlotA1 = 0;
    if(LimitSw[1] == 1)
        SlotA2 = 8;
    else if (LimitSw[1] == 0)
        SlotA2 = 0;
    if(LimitSw[2] == 1)
        SlotB1 = 2;
    else if (LimitSw[2] == 0)
        SlotB1 = 0;
    if(LimitSw[3] == 1)
        SlotB2 = 4;
    else if (LimitSw[3] == 0)
        SlotB2 = 0;

    tempAA = SlotA1+SlotA2;     // Package size A
    tempA = SlotB1+SlotB2;      // Package size B

    if(LimitSw[4] == 1)
        SlotA3 = 1;
    else if (LimitSw[4] == 0)
        SlotA3 = 0;
    if(LimitSw[5] == 1)
        SlotA4 = 8;
    else if (LimitSw[5] == 0)
        SlotA4 = 0;
    if(LimitSw[6] == 1)
        SlotB3 = 2;
    else if (LimitSw[6] == 0)
        SlotB3 = 0;
    if(LimitSw[7] == 1)
        SlotB4 = 4;
    else if (LimitSw[7] == 0)
        SlotB4 = 0;

    tempBB = SlotA3+SlotA4;
    tempB = SlotB3+SlotB4;

    if(LimitSw[8] == 1)
        SlotA5 = 1;
    else if (LimitSw[8] == 0)
        SlotA5 = 0;
    if(LimitSw[9] == 1)
        SlotA6 = 8;
    else if (LimitSw[9] == 0)
        SlotA6 = 0;
    if(LimitSw[10] == 1)
        SlotB5 = 2;
    else if (LimitSw[10] == 0)
        SlotB5 = 0;
    if(LimitSw[11] == 1)
        SlotB6 = 4;
    else if (LimitSw[11] == 0)
        SlotB6 = 0;

    tempCC = SlotA5+SlotA6;
    tempC = SlotB5+SlotB6;
}


void AGVqueueCallback(const std_msgs::Int16MultiArray& msg)
{
    queue_num = msg.data[0];
    queueid = msg.data[1];
    boxid = msg.data[2];
    drugid = msg.data[3];
    packagesize = msg.data[4];
    queueServicePoint = msg.data[5];

    if(msg.data[0] != -1)
    {
        if(queue_num != 0)
        {
            if(queue_old != queue_num)
            {
                queueid_array.push_back(msg.data[1]);
                boxid_array.push_back(msg.data[2]);
                drugid_array.push_back(msg.data[3]);
                packagesize_array.push_back(msg.data[4]);
                queueServicePoint_array.push_back(msg.data[5]);
              //  ROS_INFO("CHECKING QUEUE %d is id:%d boxid:%d drugid:%d packagesize:%d servicepointid:%d ",queue_num,queueid_array[0],boxid_array[0],drugid_array[0],packagesize_array[0],queueServicePoint_array[0]);
            }
         }
    }
    queue_old = queue_num;

    if(msg.data[0] == -1)
    {
        package_complete = 1;
        //ROS_INFO("QUEUE EMPTY");
    }
}

void door_unlock(int num)
{
    std::vector<short int> di;

    di.push_back(LED[0]);
    di.push_back(LED[1]);
    di.push_back(LED[2]);
    di.push_back(LED[3]);
    di.push_back(LED[4]);
    di.push_back(LED[5]);
    di.push_back(LED[6]);
    di.push_back(LED[7]);

   LED_msg.data = di ;
   //LED_pub.publish(LED_msg);
}

void door_lock(int num)
{
    LED[7] = 0;

    std::vector<short int> di;

    di.push_back(LED[0]);
    di.push_back(LED[1]);
    di.push_back(LED[2]);
    di.push_back(LED[3]);
    di.push_back(LED[4]);
    di.push_back(LED[5]);
    di.push_back(LED[6]);
    di.push_back(LED[7]);

   LED_msg.data = di ;
   //LED_pub.publish(LED_msg);
}

void robot_init()
{
    int count = 0;
    int path_update = 0;
    int find_stop = 0;


    LED[0] = 0;
    LED[1] = 0;    // Green center
    LED[2] = 0;    // Green front
    LED[3] = 0;    // Green
    LED[4] = 0;    // Orange top
    LED[5] = 0;    // Orange bottom
    LED[6] = 0;
    LED[7] = 0;

    std::vector<short int> di;

    di.push_back(LED[0]);
    di.push_back(LED[1]);
    di.push_back(LED[2]);
    di.push_back(LED[3]);
    di.push_back(LED[4]);
    di.push_back(LED[5]);
    di.push_back(LED[6]);
    di.push_back(LED[7]);

   LED_msg.data = di ;
   //LED_pub.publish(LED_msg);



    walkability[2][2] = 1;
    walkability[2][4] = 1;
    walkability[4][2] = 1;
    walkability[4][4] = 1;

    job_finished[0]= 0;
    job_finished[1]= 0;
    job_finished[1]= 0;


}

void ReplaceStringInPlace(std::string& subject, const std::string& search,const std::string& replace)
{
    size_t pos = 0;
    while ((pos = subject.find(search, pos)) != std::string::npos) {
         subject.replace(pos, search.length(), replace);
         pos += replace.length();
    }
}

int sorrr(int i,int j)
{
 return (i<j);
}

void getQueueforshipping()   // get station to go before shipping
{
    std::ostringstream os;
    std::string out;
    os << curlpp::options::Url("http://127.0.0.1/WebService/AGVWS.php?function=getQueueForShipping");

    out= os.str();

    std::string delimiter = "},";

         size_t pos = 0;
         std::string token;

        int co=0;

        while ((pos = out.find(delimiter)) != std::string::npos) {
            token = out.substr(0, pos);

            if(co==0)
            {
                ReplaceStringInPlace(token, "[{", "{");
                co++;
            }
            token = token+"}";

            out_array.push_back(token);

         //    std::cout << token << std::endl;
             out.erase(0, pos + delimiter.length());
         }
         ReplaceStringInPlace( out, "}]", "}");


         int package_num = out_array.size();
         int a[package_num+1];
         int a_old=0;


         for(int i=0;i<package_num;i++)
         {
             if (reader.parse( out_array[i] , value))
             {
                 std::string servicepointid_ship = value["orderservicepointid"].asString();

                 a[i] = atoi(servicepointid_ship.c_str());

                     if(a[i] != a_old)
                     {
                         destination_ship.push_back(servicepointid_ship);
                         destination_ship_int.push_back(a[i]);
                     }
                     a_old = a[i];
             }
         }

         if (reader.parse(out , value))
         {
             std::string servicepointid_ship = value["orderservicepointid"].asString();

             a[package_num] = atoi(servicepointid_ship.c_str());

                 if(a[package_num] != a_old)
                 {
                    destination_ship.push_back(servicepointid_ship);
                    destination_ship_int.push_back(a[package_num]);
                 }
                 a_old = a[package_num];

         }

         // sorting for shipping
         //std::sort(destination_ship_int.begin(),destination_ship_int.end(),sorrr);


         //for(int o=0;o<destination_ship_int.size();o++)
         //   std::cout <<  destination_ship_int[o] << endl;


}

bool mypredicate (int i, int j) {
  return (i==j);
}   // for search value in vector

int getOrderlistfromSlot()   // when take package out
{

    std::ostringstream os;
    std::string out;
    std::string shelf_number;
    std::vector<int>::iterator it;

    os << curlpp::options::Url("http://127.0.0.1/WebService/AGVWS.php?function=getOrderListfromSlot");

    out= os.str();

    if (reader.parse( out , value))
     {

         const Json::Value arrayObj = value["result"];
         for (int i=0; i<arrayObj.size(); i++)
         {
              out = arrayObj[i]["queueid"].asString();
              shelf_number = arrayObj[i]["slottypeid"].asString();

              if(package_out_array.size() == 0)
              {
                  package_out_array.push_back(atoi(out.c_str()));
                  window_out_array.push_back(atoi(shelf_number.c_str()));
              }
              else
              {
                  std::vector<int>::const_iterator it = std::find(package_out_array.begin(), package_out_array.end(), atoi(out.c_str()));

                   if (it!=package_out_array.end())
                   {
                     std::cout << "queue in vector found";
                   }
                   else
                   {
                       package_out_array.push_back(atoi(out.c_str()));
                       window_out_array.push_back(atoi(shelf_number.c_str()));
                   }

                   return arrayObj.size();
                  std::cout << arrayObj.size();
                  std::cout << " ";
                  std::cout << shelf_number;
                  if (i != arrayObj.size() - 1)
                   std::cout <<  " " << std::endl;
              }
         }
    }
}

void updateWindow(int slotnum,int status)
{
    char buf[200];
    sprintf(buf, "http://127.0.0.1/WebService/AGVWS.php?function=updatewindow&slottypeid=%d&active=%d",slotnum,status);
    request.setOpt(new curlpp::options::Url(buf));
    request.perform();  // update
}

void updateSlotAGV(int slot_id,int drug_id,int queue_id,int slottype_id)
{
    char buf[200];
    sprintf(buf, "http://127.0.0.1/WebService/AGVWS.php?function=updateslot&slotid=%d&drugid=%d&queueid=%d&slottypeid=%d",slot_id,drug_id,queue_id,slottype_id);
    request.setOpt(new curlpp::options::Url(buf));
    request.perform();  // update
}

void updateComplete(int status)
{
    char buf[200];
    sprintf(buf, "http://127.0.0.1/WebService/AGVWS.php?function=updateCompleteHIS&complete=%d",status);
    request.setOpt(new curlpp::options::Url(buf));
    request.perform();  // update

}

void updateOrderstatus(int queueid,int orderstatus)
{
    char buf[200];
    sprintf(buf, "http://127.0.0.1/WebService/AGVWS.php?function=updateQueueStatus&queueid=%d&orderstatusid=%d",queueid,orderstatus);
    request.setOpt(new curlpp::options::Url(buf));
    request.perform();  // update
}

void updateRobotdetail(int robotid,char *location,float speed,float power,float battery,int position)
{
    char buf[200];
    sprintf(buf, "http://127.0.0.1/WebService/AGVWS.php?function=updatedetailAGV&robotid=%d&location=%s&speed=%f&power=%f&positionid=%d&battery=%f",robotid,location,speed,power,position,battery);
    request.setOpt(new curlpp::options::Url(buf));
    request.perform();  // update

}
void updateDoorstatus(int robotid,int status)
{
    char buf[200];
    sprintf(buf, "http://127.0.0.1/WebService/AGVWS.php?function=updateDoorAGVstatus&robotid=%d&dooractive=%d",robotid,status);
    request.setOpt(new curlpp::options::Url(buf));
    request.perform();  // update
}

void updateEngine(int status)
{
    char buf[200];
    sprintf(buf, "http://127.0.0.1/WebService/AGVWS.php?function=updateEngine&mode=%d",status);
    request.setOpt(new curlpp::options::Url(buf));
    request.perform();  // update
}

void deleteQueuefromSlot(int queueid,int slotid)
{
    char buf[200];
    sprintf(buf, "http://127.0.0.1/WebService/AGVWS.php?function=deleteQueuefromSlot&queueid=%d&slotid=%d",queueid,slotid);
    request.setOpt(new curlpp::options::Url(buf));
    request.perform();  // update
}

void checkConnection()
{
    std::ostringstream os;
    std::string out;
    os << curlpp::options::Url("http://127.0.0.1/WebService/AGVWS.php?function=checkConnection");

    out= os.str();

    if (reader.parse( out , value))
     {
        std::string connection = value["result"].asString();

        std::cout << connection << std::endl;

    }
}


int shelf_search(int servicepointid,int queueno)    // search and push
{
      int shelf_num;
      package_count = shelfA.size()+shelfB.size()+shelfC.size();


      if(queueno == -2)   // -2 is just read data  only
      {
          if(servicepointid == tee[0])
              shelf_num = 1;
          else if (servicepointid == tee[1])
              shelf_num = 2;
          else if (servicepointid == tee[2])
              shelf_num = 3;
      }
      else
      {

          if(tee.size() == 0)
                  qno_old = 0;

          if(queueno != qno_old)
          {

              if(servicepointid_old != servicepointid)
              {
                  tee.push_back(servicepointid);
                  servicepointid_old = servicepointid;
              }

              if(tee.size() == 0)
              {
                  shelfA.push_back(queueno);
                  return shelf_num = 1;
              }

              if(tee.size() > 0)
              {
                  if(servicepointid == tee[0])
                  {
                      shelfA.push_back(queueno);
                      shelf_num = 1;
                  }
                  else if (servicepointid == tee[1])
                  {
                      shelfB.push_back(queueno);
                      shelf_num = 2;
                  }
                  else if (servicepointid == tee[2])
                  {
                      shelfC.push_back(queueno);
                      shelf_num = 3;
                  }
                  else
                  {
                      shelf_num = 0;
                  }
              }

          }
          else
              return shelf_num = 1;

          qno_old = queueno;
      }

      return shelf_num;
}

int slot_search(int shelf,int pack_size)   // for put package in to AGV robot
{
    // Limit[] == 1  is slot empty
    // return 0 is shelf full
    // return -1 is package A full
    // return -2 is package B full


    int slot_number;
    if(shelf == 1)
    {
        if(pack_size == 1)
        {
            if(LimitSw[0] == 0)
            {
                slot_number = 1;
            }
            else if(LimitSw[3] == 0)
            {
                slot_number = 4;
            }
            else
                slot_number = -1;
        }
        else if(pack_size == 2)
        {
            if(LimitSw[1] == 0)
            {
                slot_number = 2;
            }
            else if(LimitSw[2] == 0)
            {
                slot_number = 3;

            }
            else
                slot_number = -2;
        }

    }
    else if(shelf == 2)
    {
        if(pack_size == 1)
        {
            if(LimitSw[4] == 0)
            {
                slot_number = 5;

            }
            else if(LimitSw[7] == 0)
            {
                slot_number = 8;

            }
            else
                slot_number = -1;
        }
        else if(pack_size == 2)
        {
            if(LimitSw[5] == 0)
            {
                slot_number = 6;

            }
            else if(LimitSw[6] == 0)
            {
                slot_number = 7;

            }
            else
                slot_number = -2;
        }
    }
    else if(shelf == 3)
    {
        if(pack_size == 1)
        {
            if(LimitSw[8] == 0)
            {
                slot_number = 9;

            }
            else if(LimitSw[11] == 0)
            {
                slot_number = 12;

            }
            else
                slot_number = -1;
        }
        else if(pack_size == 2)
        {
            if(LimitSw[9] == 0)
            {
                slot_number = 10;

            }
            else if(LimitSw[19] == 0)
            {
                slot_number = 11;

            }
            else
                slot_number = -2;
        }
    }

    return slot_number;
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "agv_roboto");

  ros::NodeHandle nh;
  ros::NodeHandle nLED;
  ros::NodeHandle nInteracSW;
  ros::NodeHandle nLimitSW;

  ros::NodeHandle nAGV_Queue;

  ros::Subscriber qr_sub;
  ros::Subscriber agv_interac_sub;
  ros::Subscriber agv_socket_sub;
  ros::Subscriber agv_queue_sub;  // for input to socket

  ros::Publisher command_pub;


  qr_sub =  nh.subscribe("qrdata", 1, QRCallback);
  agv_interac_sub =  nInteracSW.subscribe("InteracSw", 1, InteracSwCallback);
  agv_socket_sub =  nLimitSW.subscribe("LimitSw", 1, LimitSwCallback);
  LED_pub = nLED.advertise<std_msgs::Int16MultiArray>("LED2",1);
  command_pub = nAGV_Queue.advertise<std_msgs::String>("syscommand", 1);
  agv_queue_sub = nAGV_Queue.subscribe("AGVqueue", 1,AGVqueueCallback);

  ros::Rate r(1);

  //getOrderlistfromSlot();
  //getQueueforshipping();
  //updateRobotdetail(2,1,25,52,1);
  //updateWindow(2,0);
  //checkQueueAGV(0);

  //getOrderlistfromSlot();
  //robot_init();

  std_msgs::String command_msg;

    agv_mode =0;

    agv_state = 1;

     int ssss;

     shelfA.clear();
     shelfB.clear();
     shelfC.clear();
     tee.clear();

     robot_position = 1;

int queue_old2=0;

     int queueServicePoint_old ;


     for(int a=0;a<=12;a++)
         slot_active[a] = 0;

  while(ros::ok())
  {
    command_msg.data = "AGVcheckQueueStart";
    command_pub.publish(command_msg);

    if(reset_button == 1)    // Reset
        robot_init();

    if(agv_mode == 0)    // Auto mode
    {

        if(agv_state == 0)  // waiting for put package in slot
        {
            if(LimitSw[15] == 1)
            {
                //updateDoorstatus(2,1);  // door open
                agv_state = 1;  //go to next state
                std::cout <<  "door open" << std::endl;
            }
            else if(LimitSw[15] == 0)
            {
              // updateDoorstatus(2,0);  // door close
                std::cout <<  "waiting for door open" << std::endl;
            }
        }
        else if(agv_state == 1)     // scan >> put package in slot >> update // untill full or put start button
        {

            if(LimitSw[0] == 1)
                slot_active[0] = 1;
            if(LimitSw[1] == 1)
                slot_active[1] = 1;
            if(LimitSw[2] == 1)
                slot_active[2] = 1;
            if(LimitSw[3] == 1)
                slot_active[3] = 1;


            if(slot_num == 1)
            {
                if(slot_active[0]==1)
                {

                        slot_active[0] = 2;
                        updateOrderstatus(queueid,14);
                         ros::Duration(0.5).sleep();
                        updateSlotAGV(slot_num,drugid,queueid,bb);
                        ros::Duration(0.5).sleep();
                }
                LED[0] = 1;
                LED[1] = 0;
                LED[2] = 0;
                LED[3] = 0;

            }
            else if(slot_num == 2)
            {
                if(slot_active[1]==1)
                {

                        slot_active[1] = 2;
                        updateSlotAGV(slot_num,drugid,queueid,bb);
                        ros::Duration(0.5).sleep();
                        updateOrderstatus(queueid,14);
                        ros::Duration(0.5).sleep();
                }
                LED[1] = 1;
                LED[2] = 0;
                LED[0] = 0;
                LED[3] = 0;


            }
            else if(slot_num == 3)
            {
                if(slot_active[2]==1)
                {

                        slot_active[2] = 2;
                        updateOrderstatus(queueid,14);
                        updateSlotAGV(slot_num,drugid,queueid,bb);
                        ros::Duration(0.5).sleep();
                        updateSlotAGV(slot_num,drugid,queueid,bb);
                        ros::Duration(0.5).sleep();
                }
                LED[1] = 0;
                LED[2] = 1;
                LED[0] = 0;
                LED[3] = 0;

            }
            else if(slot_num == 4)
            {
                if(slot_active[3]==1)
                {

                        slot_active[3] = 2;
                        updateOrderstatus(queueid,14);
                        updateSlotAGV(slot_num,drugid,queueid,bb);
                        ros::Duration(0.5).sleep();
                        updateSlotAGV(slot_num,drugid,queueid,bb);
                        ros::Duration(0.5).sleep();
                }

                LED[3] = 1;
                LED[0] = 0;
                LED[1] = 0;
                LED[2] = 0;
            }

            if(shelfA.size() == 4)
            {

                LED[3] = 0;
                LED[0] = 0;
                LED[0] = 0;
                LED[3] = 0;
                ROS_INFO("SHELF FULL");
            }

            if(start_button == 1)           //
            {
                agv_state = 2;
            }


            if(queueid_array.size() > 0)
            {

                /*std::cout << "Input service point : ";
                    std::cin >> aaa;
                    std::cout << "Input queue no : ";
                        std::cin >> qno;
                         bb = shelf_search(aaa,qno);
                         slot_num = slot_search(bb,packagesize);*/

                    bb = shelf_search(queueServicePoint,queue_num);
                    slot_num = slot_search(bb,packagesize);


                    if(bb != 0)
                    {

                       /* ROS_INFO("Package size : %d",packagesize);
                        ROS_INFO("Service point : %d",queueServicePoint);
                        ROS_INFO("Select shelf number : %d",bb);
                        ROS_INFO("queueid : %d",queueid);   */
                        ROS_INFO("Slot : %d",slot_num);
                        ROS_INFO("Slot active: %d",slot_active[0]);
                        if(bb == 1)
                        {
                            ROS_INFO("ShelfA size is %d",shelfA.size());
                           // door_unlock(1);

                        }
                        else if (bb == 2)
                        {
                            ROS_INFO("ShelfB size is %d",shelfB.size());
                            //door_unlock(2);
                        }
                        else if (bb == 3)
                        {
                            ROS_INFO("ShelfC size is %d",shelfC.size());
                            //door_unlock(3);
                        }

                        std::vector<short int> di;


                        di.push_back(LED[0]);
                        di.push_back(LED[1]);
                        di.push_back(LED[2]);
                        di.push_back(LED[3]);
                        di.push_back(LED[4]);
                        di.push_back(LED[5]);
                        di.push_back(LED[6]);
                        di.push_back(LED[7]);
                        di.push_back(LED[8]);
                        di.push_back(LED[9]);
                        di.push_back(LED[10]);
                        di.push_back(LED[11]);
                        di.push_back(LED[12]);
                        di.push_back(LED[13]);
                        di.push_back(LED[14]);
                        di.push_back(LED[15]);



                       LED_msg.data = di ;
                       LED_pub.publish(LED_msg);

                    }

                    else
                    {
                        ROS_INFO("EEEERRRRRRRRRRROOORR");
                    }

                    if(shelfA.size() == 4)
                    {
                        ROS_INFO("ShelfA data queue is %d %d %d %d",shelfA[0],shelfA[1],shelfA[2],shelfA[3]);

                    }

            }

        }
        else if(agv_state == 2) // waiting for start button
        {
            //agv_state=3;
            ROS_INFO("OK");
        }
        else if(agv_state == 3) // getQueueforshipping
        {
            getQueueforshipping();
            agv_state=4;

        }
        else if(agv_state == 4)  // Line tracking
        {
            int number_of_station = destination_ship_int.size();

            if(number_of_station == 0)
                agv_state = 7;
            else
            {
              if(robot_position == destination_ship_int[0]) // check begin of destination list
              {
                  std::cout <<  "   robot stop" << endl;
                  //updateEngine(0);
                  //updateComplete(0);
                  int shelf_num = shelf_search(destination_ship_int[0],-2);   // search shelf of ward destination

                  if(shelf_num == 1)
                      for(int r=0;r<shelfA.size();r++)
                          updateOrderstatus(shelfA[r],17);
                  else if(shelf_num == 2)
                      for(int r=0;r<shelfB.size();r++)
                          updateOrderstatus(shelfB[r],17);
                  else if(shelf_num == 3)
                      for(int r=0;r<shelfC.size();r++)
                          updateOrderstatus(shelfC[r],17);


                  agv_state = 5;
              }
              else
              {
                  //std::cout <<  "   Line tracking" << endl;
              }
            }
            //updateRobotdetail(2,"WARD1",0.23,54,22.43,1);
        }
        else if(agv_state == 5)   // Exporting    // check queueid for export
        {

           // if(LimitSw[15] == 1)
           // {
            int package_co;
                package_co = getOrderlistfromSlot();
                std::cout << "Total for package : ";
                std::cout << package_co << endl;
                std::cout << "Shelf for open is : ";
                if(window_out_array.size() > 0)
                    std::cout << window_out_array[0] << endl;

                //door_unlock(window_out_array[0]);

                if(package_co == 19)
                    agv_state = 6;
                //std::cout << "  door open ";

           // }
           // else if(LimitSw[15] == 0)
           // {
             //   std::cout <<  "waiting for door open" << std::endl;
           // }
        }
        else if(agv_state == 6)
        {

            std::cout <<  "Complete for exporting" << endl;
            destination_ship_int.erase(destination_ship_int.begin());  // delete queue in robot for shipping
            agv_state = 4;
        }
        else if(agv_state == 7)   // go back to station
        {
            std::cout <<  "no queue going back to station  LalllallallllaLaaaaaaaa" << endl;
        }

      }//AGV mode 0(autonomous)
      else if(agv_mode == 1)  // Manual mode
      {
                    /*  LED[3] = 0;    // Green
                      LED[5] = 1;    // Orange bottom

                      std::vector<short int> di;

                      di.push_back(LED[0]);
                      di.push_back(LED[1]);
                      di.push_back(LED[2]);
                      di.push_back(LED[3]);
                      di.push_back(LED[4]);
                      di.push_back(LED[5]);
                      di.push_back(LED[6]);
                      di.push_back(LED[7]);
                    start_enable  = 0;
                     LED_msg.data = di ;
                     LED_pub.publish(LED_msg); */
                        start_enable = 0;
                      ROS_INFO("MANUAL MODE");

      }//AGV mode 1
      ros::spinOnce();
   }//while(ros::ok())


}// int main()


